#include "bsp_as5600.h"

#define abs(x) ((x)>0?(x):-(x))
#define _2PI 6.28318530718

static uint16_t angle_data_prev; //上次位置
static float full_rotation_offset; //转过的整圈数

/// @brief 磁编码器初始化
/// @param  获取初始角度以及清除累计圈数
void bsp_as5600Init(void)
{
  full_rotation_offset = 0;
  angle_data_prev = bsp_as5600GetRawAngle();
}
/// @brief iic写数据
/// @param dev_addr 设备地址
/// @param pData 发送的数据
/// @param count 长度
/// @return 
uint16_t status0;
static uint16_t i2cWrite(uint8_t dev_addr, uint8_t* pData, uint32_t count)
{
   static uint16_t status;
  status = HAL_I2C_Master_Transmit(&AS5600_I2C_HANDLE, dev_addr, pData, count,1);
  return status;
}
/// @brief iic读取数据
/// @param dev_addr 设备地址
/// @param pData 存放数据的地址
/// @param count 数据长度
/// @return  
uint16_t status1;
static uint16_t i2cRead(uint8_t dev_addr, uint8_t* pData, uint32_t count)
{
  static uint16_t status;
  status = HAL_I2C_Master_Receive(&AS5600_I2C_HANDLE, (dev_addr | 1), pData, count,1);
  return status;
}

/// @brief 读取原始数据
/// @param  void 
/// @return angle(0-4096)
uint16_t bsp_as5600GetRawAngle(void)
{
  uint16_t raw_angle;
  uint8_t buffer[2] = { 0 };
  uint8_t raw_angle_register = AS5600_RAW_ANGLE_REGISTER;
  status1=i2cWrite(AS5600_ADDR, &raw_angle_register, 1);
  status0=i2cRead(AS5600_ADDR, buffer, 2);
  raw_angle = ((uint16_t)buffer[0] << 8) | (uint16_t)buffer[1];
  return raw_angle;
}

/// @brief  获取磁编码器的角度
/// @param  void
/// @return dealate angle(绝对角度)
float getAngle(void)
{
  uint16_t angle_data = bsp_as5600GetRawAngle();
  int16_t d_angle = angle_data - angle_data_prev;
  if (abs(d_angle) > 2048)
  {
    full_rotation_offset += (d_angle > 0 ? -_2PI : _2PI);
  }
  angle_data_prev = angle_data;
  return (full_rotation_offset + (angle_data / 4096.0f) * _2PI);
}



